package es.uji.viselab.kinematics;

import javax.vecmath.Matrix4f;

import es.uji.viselab.math.Joints;
import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.PointXYZ;
import es.uji.viselab.math.Pose;
import es.uji.viselab.math.RobotToolLocation;
import es.uji.viselab.robot.RobotCatalog;
import es.uji.viselab.robot.RobotDatabase;

/**
 * You can create an instance of this class with the Denavit-Hartenberg
 * parameters. After that, is is possible to get the position (xyz) of the
 * end-effector based on the joint angles.
 * 
 * @author Roger Esteller-Curto
 * @see Kparameter
 */
public class DKinematic {

	// Denavit-Hartenberg parameters for direct kinematics
	private Kparameter[] kParam;
	private int numParams;
	private RobotCatalog robotModel;

	/**
	 * Constructor
	 * 
	 * @param type
	 *            RobotCatalog type to get the Denavit-Hartembert parameters
	 */
	public DKinematic(RobotCatalog type) {
		robotModel = type;
		kParam = RobotDatabase.getDHParameters(robotModel);
		numParams = RobotDatabase.getNumberJoints(robotModel);
	}

	public TMatrix4f getDirectKinematicMatrix(Joints joints) {
		// Use this if you want to know the complete matrix transform
		// is the same that: where is the robot hand, and what are the sliding
		// and approach vectors ?

		TMatrix4f matrix = new TMatrix4f();
		joints.setJoint3(joints.getJoint(3)+joints.getJoint(2));
		// For each of the DH parameters, we multiply the matrix
		for (int i = numParams; i > 0; i--) {
			matrix.rotX(kParam[i - 1].alfa);
			matrix.trans(kParam[i - 1].a, 0, 0);
			if (i == 2 || i == 3)
				matrix.rotZ(kParam[i - 1].tecta - joints.getJoint(i) * (Math.PI * 2) / 360);
			else
				matrix.rotZ(kParam[i - 1].tecta + joints.getJoint(i) * (Math.PI * 2) / 360);
			matrix.trans(0, 0, kParam[i - 1].d);
		}
		return matrix;
	}

	public RobotCatalog getRobotModel() {
		return robotModel;
	}

	public int getNumberJoints() {
		return numParams;
	}

	/**
	 * Returns the RobotToolLocation (position+orientation) of the end-effector
	 * of the robot
	 * 
	 * @param j
	 *            Joints
	 * @return RobotToolLocation
	 */
	public RobotToolLocation getRtl(Joints j) {

		RobotToolLocation rtl = new RobotToolLocation();
		TMatrix4f matrix = this.getDirectKinematicMatrix(j);
		PointXYZ pointXYZ = new PointXYZ(0, 0, 0);
		pointXYZ.transform(matrix);
		rtl.setLocation(pointXYZ);
		PointXYZ vector = new PointXYZ(0, 0, 1);
		vector.transform(matrix);
		vector.substract(pointXYZ);
		rtl.setOrientation(vector);
		return rtl;
	}

	public Matrix4 getDirectKinematicMatrix(double[] joints) {
		// Use this if you want to know the complete matrix transform
		// is the same that: where is the robot hand, and what are the sliding
		// and approach vectors ?

		Matrix4 matrix = new Matrix4();
		matrix.setIdentity();
		joints[2]=joints[2]+joints[1];

		// For each of the DH parameters, we multiply the matrix
		for (int i = numParams - 1; i >= 0; i--) {

			Matrix4 transX = Matrix4.getTranslation(kParam[i].a, 0, 0);
			Matrix4 transZ = Matrix4.getTranslation(0, 0, kParam[i].d);
			Matrix4 rotationZ = new Matrix4();
			rotationZ.rotZ(kParam[i].tecta + joints[i]);
			Matrix4 rotationX = new Matrix4();
			rotationX.rotX(kParam[i].alfa);

			rotationX.mul(matrix);
			transX.mul(rotationX);
			transZ.mul(transX);
			rotationZ.mul(transZ);
			matrix = rotationZ;

		}
		return matrix;
	}

	public Pose getPose(Joints j) {

		double[] v = new double[6];
		for (int i = 0; i < 6; i++)
			v[i] = j.getJoint(i);
		 Matrix4 matrix = this.getDirectKinematicMatrix(v);
		 
		 /*TMatrix4f matrix = new TMatrix4f(matrix);
		PointXYZ pointXYZ = new PointXYZ(0, 0, 0);
		pointXYZ.transform(matrix);

		PointXYZ vector = new PointXYZ(0, 0, 1);
		vector.transform(matrix);
		vector.substract(pointXYZ);

		Pose pose = new Pose(pointXYZ.getX(), pointXYZ.getY(), pointXYZ.getZ(), vector.getX(),
				vector.getY(), vector.getZ());  */
        Pose pose = new Pose(matrix.m03, matrix.m13, matrix.m23, 0,0,0);				
		return pose;
	}
}
